// -*- c++ -*-
/**************************************************************/
/*  This file is licensed from, and is a trade secret of:     */
/*                                                            */
/*    Mercury Computer Systems, Incorporated                  */
/*    199 Riverneck Road                                      */
/*    Chelmsford, Massachusetts 01824-2820                    */
/*    United States of America                                */
/*    Telephone + 1 978 256-1300                              */
/*                                                            */
/*  Refer to your Software License Agreement for provisions   */
/*  on use, duplication, or disclosure.                       */
/*  US. Government users and prime contractors are also       */
/*  governed by the following Data Rights Notice:             */
/*  Use, duplication or disclosure by the US. Government is   */
/*  subject to the provisions set forth in FAR 52.227-14 for  */
/*  restricted rights or other agency data rights provisions  */
/*  on commercial software and documentation, as applicable.  */
/*                                                            */
/*  This file: (a) was developed at private expense, and no   */
/*  part was developed with government funds, (b) is a        */
/*  trade secret of Mercury Computer Systems, Inc. for the    */
/*  purposes of the Freedom of Information Act, (c) is        */
/*  "commercial computer software" subject to limited         */
/*  utilization as provided in the above noted Software       */
/*  License Agreement and (d) is in all respects data         */
/*  belonging exclusively to Mercury Computer Systems, Inc..  */
/*                                                            */
/**************************************************************/

/******************************************************************/
/* Copyright (c) 1999-2003, Mercury Computer Systems, Inc.,       */
/* Chelmsford MA.,and all third party embedded software sources.  */
/* All rights reserved under the Copyright laws of US. and        */
/* international treaties.                                        */
/*                                                                */
/* Patent Pending                                                 */
/******************************************************************/


//$COPYRIGHT_END


#include <iostream>
#include <math.h>
#include <assert.h>
#include "cvsd.h"

/*
 * CVSD constants.
 */

const double CVSD::AD_MAX         = 32767.0f;       // A/D Max voltage
const double CVSD::DA_MAX         = 32767.0f;       // D/A Max Voltage
const double CVSD::CVSD_STEP_MAX  = DA_MAX / 4.0f;  // Max step size for CVSD Encoding
const double CVSD::CVSD_STEP_MIN  = DA_MAX / 128.0f;// Min step size for CVSD Encoding
const double CVSD::CVSD_DELTA_TC  = 0.015625f;      // Integration time constant for CVSD delta
const double CVSD::CVSD_REF_TC    = 0.0625f;        // Integration time constant for CVSD Encoding
const double CVSD::ATTACK_AGC_TC  = 0.5f;           // Weighted time constant for attack AGC
const double CVSD::RELEASE_AGC_TC = 0.02f;          // Weighted time constant for release AGC
const double CVSD::MAX_AGC_RANGE  = DA_MAX / 4.0f;  // Set Max to 1/4 of full scale D/A value

const int CVSD::FRAME_BLK_SIZE = 320;
const int CVSD::FILTER_LENGTH  = 51; // Low Pass Filter order for up/down Sampling

extern "C" {
  CVSD_EXPORT
  POA_CF::Resource *
  CreateCVSDEncoder (const char * identifier)
  {
    CVSDEncoder * Encoder;

    if (CVSDEncoder::Create (Encoder, identifier) != success) {
      return 0;
    }

    return (POA_CF::Resource *) Encoder;
  }

  CVSD_EXPORT
  POA_CF::Resource *
  CreateCVSDDecoder (const char *identifier)
  {
    CVSDDecoder * Decoder;

    if (CVSDDecoder::Create (Decoder, identifier) != success) {
      return 0;
    }

    return (POA_CF::Resource *) Decoder;
  }
}

/*
 * ----------------------------------------------------------------------
 * CVSD
 * ----------------------------------------------------------------------
 */

CVSD::CVSD (const char * identifier)
  : CF__Resource_impl (identifier),
    m_vlPCMData (0),
    m_vfPCMData (0),
    m_vlCVSDData (0),
    m_vfCVSDData (0),
    m_threadrunning (false)
{
  m_vfPCMData = new float [FRAME_BLK_SIZE];
  m_vfCVSDData = new float [FRAME_BLK_SIZE+FILTER_LENGTH];
}

CVSD::~CVSD ()
{
  m_thread.detach();

  delete m_vlPCMData;
  delete m_vfPCMData;
  delete m_vlCVSDData;
  delete m_vfCVSDData;
}

status
CVSD::Init ()
{
  m_fAverageRMS = 0.0;
  m_iOldbits = 0;
  m_fOld_avg_delta = 0.0;
  m_fOld_cvsd_ref = 0.0;
  m_iLineNumber = 0;
  return success;
}

// Generic thread OS signature which calls component specific thread function.
void
CVSD::RunWorker (void * param)
{
  CVSD * component =
    (CVSD *) param;

  component->RunLoop ();
} // End of CVSD::RunWorker (void * param)

/* Coefficients for low pass filter algorithm. */
const double
CVSD::m_coCoef[51] = {
  0.0026868866f,
  0.0006820376f,
  -0.0030727922f,
  -0.0032504199f,
  0.0024948977f,
  0.0055995793f,
  -0.0005578898f,
  -0.0078069903f,
  -0.0037212502f,
  0.0085197836f,
  0.0096489396f,
  -0.0056197005f,
  -0.0162840914f,
  -0.0016015832f,
  0.0204732753f,
  0.0142358113f,
  -0.0195537023f,
  -0.0306247957f,
  0.0086750304f,
  0.0490698069f,
  0.0173309185f,
  -0.0654983670f,
  -0.0749460831f,
  0.0772542581f,
  0.3071701527f,
  0.4186406732f,
  0.3071701527f,
  0.0772542581f,
  -0.0749460831f,
  -0.0654983670f,
  0.0173309185f,
  0.0490698069f,
  0.0086750304f,
  -0.0306247957f,
  -0.0195537023f,
  0.0142358113f,
  0.0204732753f,
  -0.0016015832f,
  -0.0162840914f,
  -0.0056197005f,
  0.0096489396f,
  0.0085197836f,
  -0.0037212502f,
  -0.0078069903f,
  -0.0005578898f,
  0.0055995793f,
  0.0024948977f,
  -0.0032504199f,
  -0.0030727922f,
  0.0006820376f,
  0.0026868866f
};

/*
 * ----------------------------------------------------------------------
 * CVSD Encoder
 * ----------------------------------------------------------------------
 */

status
CVSDEncoder::Create (CVSDEncoder *& handle,
		     const char *identifier)
{
  CVSDEncoder * Temp = new CVSDEncoder (identifier);

  if (!Temp) {
    handle = 0;
    return fail;
  }

  handle = Temp;
  return success;
}

CVSDEncoder::CVSDEncoder (const char *identifier)
  : CVSD (identifier)
{
  m_pcmin = new PcmInPort_impl (FRAME_BLK_SIZE);
  m_vlCVSDData = new char [FRAME_BLK_SIZE];
  m_cvsdout = new CvsdOutPort_impl (m_vlCVSDData, FRAME_BLK_SIZE);
  m_log = LogService::Log::_nil ();
  m_logportservant = new CVSDEncoderLogPort (this);
  m_logport = m_logportservant->_this ();
}

CVSDEncoder::~CVSDEncoder ()
{
  delete m_pcmin;
  delete m_cvsdout;
}

CORBA::Object_ptr
CVSDEncoder::getPort (const CORBA::Char* name)
  throw (CORBA::SystemException,
	 CF::PortSupplier::UnknownPort)
{
  std::string temp(name);

  if (temp == "CVSDEncoderIn") {
    return m_pcmin->getPort();
  }
  else if (temp == "LogPort") {
    return CF::Port::_duplicate (m_logport);
  }
  else if (temp == "CVSDEncoderOut") {
    return m_cvsdout->getPort();
  }
  else {
    throw CF::PortSupplier::UnknownPort();
  }
} // End of CVSDEncoder::getPort(const CORBA::Char* name)

void
CVSDEncoder::start ()
  throw (CORBA::SystemException,
	 CF::Resource::StartError)
{
  if (m_threadrunning) {
    return;
  }

  m_threadrunning = true;
  m_thread.start (RunWorker, (void *) this);
  CF__Resource_impl::start();
} // End of void CVSDEncoder::start ()

void
CVSDEncoder::stop ()
  throw (CORBA::SystemException,
	 CF::Resource::StopError)
{
  m_bStop = true;
  m_threadrunning = false;
  m_thread.detach ();
  m_pcmin->ReleaseDataBuffer();
} // End of void CVSDEncoder::stop ()

status
CVSDEncoder::Init ()
{
#ifdef MCS_VERBOSE
  std::cout << "CVSDEncoder: Initializing." << std::endl;
#endif

  // Example log entry.
  LogService::Log::ProducerLogRecordSequence logRecordSequence;
  logRecordSequence.length (1);
  const CORBA::ULong i = 0;
  logRecordSequence[i].producerId = this->identifier ();
  logRecordSequence[i].producerName = "CVSD Encoder";
  logRecordSequence[i].level = LogService::PROGRAMMER_DEBUG1;
  logRecordSequence[i].logData = "CVSD Encoder Component Initialized";

  if (!CORBA::is_nil(m_log)) {
    try {
      m_log->writeRecords (logRecordSequence);
    }
    catch (...) {
    }
  }

  m_bStop = false;
  return CVSD::Init();
}

// Inner loop function that checks for a stop command
void
CVSDEncoder::RunLoop ()
{
  // initialize run conditions
  if (Init () != success) {
    return;
  }

#ifdef MCS_VERBOSE
  std::cout << "CVSDEncoder: Component is running." << std::endl;
#endif

  while (!m_bStop) {
    Run ();
  }

#ifdef MCS_VERBOSE
  std::cout << "CVSDEncoder: Component is stopping." << std::endl;
#endif
} // End of CVSDEncoder::RunLoop()

void
CVSDEncoder::ConvertToCVSD (int length)
{
  AutomaticGainControl (length);
  Encode (length);
}

void
CVSDEncoder::AutomaticGainControl (int length)
{
  double sum = 0.0, temp;
  int i;

  for (i=0; i<length; i++) {
    temp = m_vlPCMData[i];
    sum += temp*temp;
  }

  double instantaneousRMS = sqrt (sum/length);

  if (instantaneousRMS > m_fAverageRMS) {
    /*
     * Attack Time of AGC i.e. initial time when first data set comes.
     */
    m_fAverageRMS += ATTACK_AGC_TC * (instantaneousRMS - m_fAverageRMS);
  }
  else {
    /* Release Time of AGC */
    m_fAverageRMS += RELEASE_AGC_TC * (instantaneousRMS - m_fAverageRMS);
  }

  /* Estimate Gain Range = 30 db. */
  double gain = (15.0 * m_fAverageRMS < MAX_AGC_RANGE) ? 15.0 : (MAX_AGC_RANGE / m_fAverageRMS);

  /* Multiply Input array by Gain. */

  for (i=0; i<length; i++) {
    m_vfPCMData[i] = static_cast<float> (m_vlPCMData[i] * gain);
  }
} 

void
CVSDEncoder::Encode (int length)
{
  unsigned int bits, three_bits;
  double error;
  double step;
  int bit, i;

  bits = m_iOldbits;          /* Bring saved bit stream to register. */

  for (i=0; i<length; i++) {
    /* Compare input with integrator output and form error signal. */
    error = m_vfPCMData[i] -  m_fOld_cvsd_ref;

    /*
     * Put bit in Bit Stream LSB.
     * Positive error gives 0 bit.
     * Negative error gives 1 bit.
     */

    if (error < 0.0) {
      bit = 1;
    }
    else {
      bit = 0;
    }

    assert (bit < 256);
    m_vlCVSDData[i] = static_cast<char> (bit);
    bits = (bits << 1) | bit;

    /*
     * Isolate and test last three bits in bit stream.
     * If not 3 in a row, decrease delta toward minimum;
     * else increase delta toward maximum.
     */

    three_bits = bits & 0x7;
    step = ((three_bits == 0) || (three_bits == 7)) ? 
      CVSD_STEP_MAX : CVSD_STEP_MIN;

    /* Low Pass Filter delta */
    m_fOld_avg_delta += CVSD_DELTA_TC * (step - m_fOld_avg_delta);

    /* Modify integrator based on sign of error signal. */
    m_fOld_cvsd_ref += (error < 0.0) ? 
      -m_fOld_avg_delta : +m_fOld_avg_delta;

    /* Low Pass Filter integrator for CVSD Ref. */
    m_fOld_cvsd_ref *= (1.0 - CVSD_REF_TC);
  }

  m_iOldbits = bits; /* Save register value bits in static memory. */
}

// Inner loop method that performs component business logic
status
CVSDEncoder::Run ()
{
#ifdef MCS_VERBOSE
  std::cout << "CVSDEncoder: Waiting for data." << std::endl;
#endif

  m_input_count = m_pcmin->GetData (&m_vlPCMData);

  if (m_input_count == 0) {
    return fail;
  }

  // Make sure we didn't overflow buffer.
  assert (m_input_count <= FRAME_BLK_SIZE);

#ifdef MCS_VERBOSE
  std::cout << "CVSDEncoder: Received " << m_input_count << " samples." << std::endl;
#endif

  ConvertToCVSD (m_input_count);

  // Don't need to reassert here, m_cvsdout is same size as m_pcmin.
  m_cvsdout->pushData (m_vlCVSDData, m_input_count);
  m_pcmin->ReleaseDataBuffer();
  return success;
} // End of CVSDEncoder::Run()

// Function to configure the Log Service.
status
CVSDEncoder::setLogObject (CORBA::Object_ptr logObject)
{
  m_log = LogService::Log::_narrow (logObject);

  if (CORBA::is_nil (m_log.in())) {
    return fail;
  }

#ifdef MCS_VERBOSE
  cout << "CVSDEncoder: Log object set." << endl;
#endif

  return success;
}

/*
 * ----------------------------------------------------------------------
 * CVSD Decoder
 * ----------------------------------------------------------------------
 */

status
CVSDDecoder::Create (CVSDDecoder *& handle,
		     const char * identifier)
{
  CVSDDecoder * Temp = new CVSDDecoder (identifier);

  if (!Temp) {
    handle = 0;
    return fail;
  }

  handle = Temp;
  return success;
}

CVSDDecoder::CVSDDecoder (const char *identifier)
  : CVSD (identifier)
{
  m_cvsdin = new CvsdInPort_impl (FRAME_BLK_SIZE);
  m_vlPCMData = new short[FRAME_BLK_SIZE];
  m_pcmout = new PcmOutPort_impl (m_vlPCMData, FRAME_BLK_SIZE);
}

CVSDDecoder::~CVSDDecoder()
{
  delete m_cvsdin;
  delete m_pcmout;
}

CORBA::Object_ptr
CVSDDecoder::getPort (const CORBA::Char* name)
  throw (CORBA::SystemException,
	 CF::PortSupplier::UnknownPort)
{
  std::string temp(name);

  if (temp == "CVSDDecoderIn") {
    return m_cvsdin->getPort();
  }
  else if (temp == "CVSDDecoderOut") {
    return m_pcmout->getPort();
  }
  else {
    throw CF::PortSupplier::UnknownPort();
  }
} // End of CVSDDecoder::getPort(const CORBA::Char* name)

void
CVSDDecoder::start ()
  throw (CORBA::SystemException, CF::Resource::StartError)
{
  if (m_threadrunning) {
    return;
  }

  m_threadrunning = true;
  m_thread.start (RunWorker, (void *) this);
  CF__Resource_impl::start();
} // End of void CVSDDecoder::start ()

void
CVSDDecoder::stop ()
  throw (CORBA::SystemException,
	 CF::Resource::StopError)
{
  m_bStop = true;
  m_threadrunning = false;
  m_thread.detach();
  m_cvsdin->ReleaseDataBuffer();
} // End of void CVSDDecoder::stop ()

status
CVSDDecoder::Init ()
{
#ifdef MCS_VERBOSE
  std::cout << "CVSDDecoder: Initializing." << std::endl;
#endif

  /*
   * Init Filter at each new "push to talk".
   */

  m_bStop = false;

  for (int i=0; i<FILTER_LENGTH; i++) {
    m_vfCVSDData[i] = 0;
  }

  return CVSD::Init();
}

// Inner loop function that checks for a stop command
void
CVSDDecoder::RunLoop ()
{
  m_bStop = false;

  // initialize run conditions
  if (Init () != success) {
    return;
  }

#ifdef MCS_VERBOSE
  std::cout << "CVSDDecoder: Component is running." << std::endl;
#endif

  while (!m_bStop) {
    Run ();
  }

#ifdef MCS_VERBOSE
  std::cout << "CVSDDecoder: Component is stopping." << std::endl;
#endif
} // End of CVSDDecoder::RunLoop()

void
CVSDDecoder::ConvertToPCM (int length)
{
  Decode (length);
  LowPassFilter (length);
}

void
CVSDDecoder::Decode (int length)
{
  unsigned int bits, three_bits, bit;
  double step;
  int i;

  bits = m_iOldbits;

  for (i=0; i<length; i++) {
    bit = (m_vlCVSDData[i]==0) ? 0 : 1;
    bits = (bits <<= 1) | bit;

    /*
     * Isolate and test last three bits in Bit Stream.
     * If not 3 in a row, decrease delta toward minimum;
     * else increase delta toward maximum.
     */

    three_bits = bits & 0x7;
    step = ((three_bits == 0) || (three_bits == 7)) ? 
      CVSD_STEP_MAX : CVSD_STEP_MIN;

    /* Low Pass Filter delta with time constant CDEL. */
    m_fOld_avg_delta += CVSD_DELTA_TC * (step - m_fOld_avg_delta);

    /* Modify integrator based on last data bit. */
    m_fOld_cvsd_ref += m_vlCVSDData[i] ? -m_fOld_avg_delta : +m_fOld_avg_delta;

    /* Low Pass Filter integrator with time constant CINT. */
    m_fOld_cvsd_ref *= (1.0 - CVSD_REF_TC);

    m_vfCVSDData[FILTER_LENGTH+i] = static_cast<float> (m_fOld_cvsd_ref);
  }

  m_iOldbits = bits; /* Save register value bits in static memory. */
}

void
CVSDDecoder::LowPassFilter (int length)
{
  double sum;
  int i, j;

  for (i=0; i<length; i++) {
    sum = 0;

    for (j=0; j<FILTER_LENGTH; j++) {
      sum += m_vfCVSDData[i+j] * m_coCoef[j];
    }

    m_vlPCMData[i] = (short) sum;
  }

  memcpy (m_vfCVSDData, m_vfCVSDData+length, FILTER_LENGTH*sizeof(float));
}

// Inner loop method that performs component business logic
status
CVSDDecoder::Run ()
{
#ifdef MCS_VERBOSE
  std::cout << "CVSDDecoder: Waiting for data." << std::endl;
#endif

  m_input_count = m_cvsdin->GetData (&m_vlCVSDData);

  if (m_input_count == 0) {
    return fail;
  }

  // Make sure we didn't overflow buffer.
  assert (m_input_count <= FRAME_BLK_SIZE);

#ifdef MCS_VERBOSE
  std::cout << "CVSDDecoder: Received " << counter << " bits." << std::endl;
#endif

  ConvertToPCM (m_input_count);

  // Don't need to reassert here, m_pcmout is same size as m_cvsdin.
  m_pcmout->pushData (m_vlPCMData, m_input_count);
  m_cvsdin->ReleaseDataBuffer();
  return success;
} // End of CVSDDecoder::Run()

/*
 * ----------------------------------------------------------------------
 * CVSD Encoder Log Port
 * ----------------------------------------------------------------------
 */

CVSDEncoderLogPort::CVSDEncoderLogPort (CVSDEncoder* parent)
  : m_parentComponent (parent)
{
};

CVSDEncoderLogPort::~CVSDEncoderLogPort()
{
};

void
CVSDEncoderLogPort::connectPort (CORBA::Object_ptr connection,
				 const CORBA::Char * connectionId)
  throw (CORBA::SystemException,
	 ::CF::Port::InvalidPort,
	 ::CF::Port::OccupiedPort)
{
  if (CORBA::is_nil (connection)) {
    CF::Port::InvalidPort ex;
    ex.errorCode = 1;
    ex.msg = "Connection Object is a Nil CORBA Object Reference";
    throw ex;
  }

  if (m_parentComponent->setLogObject (connection) == false) {
    CF::Port::InvalidPort ex;
    ex.errorCode = 1;
    ex.msg = "Connection Object is not a Log Object Reference";
    throw ex;
  }
  
  m_connectionId = CORBA::string_dup (connectionId);

#ifdef MCS_VERBOSE
  cout << "CVSEncoderLogPort: Connected CVSDLogPort to log service "
       << "using connection ID \"" << m_connectionId << "\"" << endl;
#endif
}

void
CVSDEncoderLogPort::disconnectPort (const CORBA::Char* connectionId)
  throw (CORBA::SystemException,
	 ::CF::Port::InvalidPort)
{
  if (strcmp (m_connectionId.in(), connectionId) == 0) {
    // Disconnect.
    m_parentComponent->setLogObject (CORBA::Object::_nil());
  }
  else {
    CF::Port::InvalidPort ex;
    ex.errorCode = 2;
    ex.msg = "Disconnect: ConnectionId is not valid";
    throw ex;
  }
}
